/*
 * controller.cpp
 *
 *  Created on: Nov 26, 2012
 *      Author: michiel
 */

#include "PS3Controller/controller.h"

void Controller::init() {
	eyes_pub_		= nh_.advertise<comm::eyes>(Topic::EMO_HANDLER_EYES, 1);
	trunk_pub_		= nh_.advertise<comm::trunk>(Topic::EMO_HANDLER_TRUNK, 1);
	eyebrows_pub_	= nh_.advertise<comm::eyebrows>(Topic::EMO_HANDLER_BROW, 1);
	ears_pub_		= nh_.advertise<comm::ears>(Topic::EMO_HANDLER_EARS, 1);
}

void Controller::spin() {
	ros::Rate loop_rate(30);

	int earstate			= EARS_NEUTRAL;

	while (ros::ok()) {
		comm::eyes		messageEyes;
		comm::ears		messageEars;
		comm::eyebrows	messageEyebrows;
		comm::trunk		messageTrunk;

		messageEyes.leftpitch	= leftStickPositionYAxis_;
		messageEyes.leftyaw		= leftStickPositionXAxis_;
		messageEyes.rightpitch	= rightStickPositionYAxis_;
		messageEyes.rightyaw	=	rightStickPositionXAxis_;

		messageEars.leftstate	= earstate;
		messageEars.rightstate	= earstate;
		messageEars.leftangle	= leftTrigger_;
		messageEars.rightangle	= rightTrigger_;

		messageEyebrows.state	= eyebrowstate_;
		messageEyebrows.angle	= 0.1;

		messageTrunk.state		= trunkstate_;
		messageTrunk.angle		= 0.25;

		eyes_pub_.publish(messageEyes);
		trunk_pub_.publish(messageTrunk);
		ears_pub_.publish(messageEars);
		eyebrows_pub_.publish(messageEyebrows);

		ros::spinOnce();
		loop_rate.sleep();
	}

}

void Controller::controllerCallback(const sensor_msgs::Joy::ConstPtr &message) {
	axes_			= message->axes;
	buttons_	= message->buttons;

	leftStickPositionXAxis_		= axes_[PS3_AXIS_STICK_LEFT_LEFTWARDS];
	leftStickPositionYAxis_		= axes_[PS3_AXIS_STICK_LEFT_UPWARDS];
	rightStickPositionXAxis_	= axes_[PS3_AXIS_STICK_RIGHT_LEFTWARDS];
	rightStickPositionYAxis_	= axes_[PS3_AXIS_STICK_RIGHT_UPWARDS];

	leftTrigger_		= axes_[PS3_AXIS_BUTTON_REAR_LEFT_2] * -1;
	rightTrigger_		= axes_[PS3_AXIS_BUTTON_REAR_RIGHT_2] * -1;

	buttonUp_		=	buttons_[PS3_BUTTON_CROSS_UP];
	buttonDown_		=	buttons_[PS3_BUTTON_CROSS_DOWN];
	buttonCross_	=	buttons_[PS3_BUTTON_ACTION_CROSS];
	buttonTriangle_	=	buttons_[PS3_BUTTON_ACTION_TRIANGLE];

	if (!upPressed_ && buttonUp_ == 1) {
		browStatePrev();
		upPressed_ = true;
	} else if (upPressed_ && buttonUp_ == 0) {
		upPressed_ = false;
	}

	if (!downPressed_ && buttonDown_ == 1) {
		browStateNext();
		downPressed_ = true;
	} else if (downPressed_ && buttonDown_ == 0) {
		downPressed_ = false;
	}

	if (!trianglePressed_ && buttonTriangle_ == 1) {
		trunkStatePrev();
		trianglePressed_ = true;
	} else if (trianglePressed_ && buttonTriangle_ == 0) {
		trianglePressed_ = false;
	}

	if (!crossPressed_ && buttonCross_ == 1) {
		trunkStateNext();
		crossPressed_ = true;
	} else if (crossPressed_ && buttonCross_ == 0) {
		crossPressed_ = false;
	}
}

void Controller::browStateNext() {
	switch (eyebrowstate_) {
		case EYEBROWS_DOWN: {
			eyebrowstate_ = EYEBROWS_NEUTRAL;
			break;
		}
		default: {
			eyebrowstate_ = EYEBROWS_UP;
			break;
		}
	}
}

void Controller::browStatePrev() {
	switch (eyebrowstate_) {
		case EYEBROWS_UP: {
			eyebrowstate_ = EYEBROWS_NEUTRAL;
			break;
		}
		default: {
			eyebrowstate_ = EYEBROWS_DOWN;
			break;
		}
	}
}

void Controller::trunkStateNext() {
	switch (trunkstate_) {
		case TRUNK_DOWN: {
			trunkstate_ = TRUNK_NEUTRAL;
			break;
		}
		default: {
			trunkstate_ = TRUNK_UP;
			break;
		}
	}
}

void Controller::trunkStatePrev() {
	switch (trunkstate_) {
		case TRUNK_UP: {
			trunkstate_ = TRUNK_NEUTRAL;
			break;
		}
		default: {
			trunkstate_ = TRUNK_DOWN;
			break;
		}
	}
}

int main(int argc, char** argv) {
	ros::init(argc, argv, "ps3_controller_demo");

	Controller controller;
	controller.init();
	controller.spin();
}
